!;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; Spatial sorting using octrees.
;;;;

(in-package :dreaming-tree)

(defclass octree (node)
  ((halfwidth :accessor halfwidth :initform 0.0 :initarg :halfwidth)
   (depth :accessor max-depth :initform 6 :initarg :max-depth)))

(defmethod initialize-instance :after ((obj octree) &rest initargs)
  (declare (ignore initargs))
  (with-slots (halfwidth) obj
    (setf (bounding-volume obj) (make-instance 'bounding-box :extent (vector )))))

(defstruct onode
  (center (zero-vector 3) :type vector)
  (half-width 1.0 :type single-float)
  (has-child 0 :type fixnum)
  (children (make-array 8) :type array)
  olist)

(defmethod visible-p ((cull basic-culler) (on onode))
  (visible-p cull (make-instance ') (bounding-volume n)))

(defun make-octree (center width &optional (depth 6))
  (labels ((%build-tree (c w depth)
	     (cond ((< depth 0) nil)
		   (t (let ((n (make-onode :center c :half-width w))
			    (step (* 0.5 w)))
			(dotimes (i 8)
			  (let* ((x (if (= 1 (logand i 1)) step (- step)))
				 (y (if (= 2 (logand i 2)) step (- step)))
				 (z (if (= 4 (logand i 4)) step (- step)))
				 (o (vector x y z)))
			    (setf (aref (onode-children n) i) (%build-tree (m+ c o) step (1- depth)))))
			n)))))
    (make-instance 'octree 
		   :transformation (make-transform :b center)
		   :halfwidth (* 0.5 width) :max-depth depth
		   :children (%build-tree center (* 0.5 width) depth))))

(defmethod insert-object ((s spatial) (ot octree))
  (if (every #'(lambda (x)
		 (< x (radius ot)))
	     (center s))
      (insert-object s (children ot))
      (warn "Object ~A center (~A) lies outside bounding volume of tree ~A (center=~A,radius=~A). Not inserting.~%" s (center s) ot (center ot) (radius ot))))

(defmethod insert-object ((s spatial) (n onode))
  (format t "Inserting object at ~A.~%" (center s))
  (let ((straddle nil)
	(index 0))
    (dotimes (i 3)
      (let ((delta (- (mref (center s) i) (mref (onode-center n) i))))
	(if (<= (abs delta) (radius s))
	    (setf straddle t))
	(if (> delta 0.0)
	    (setf index (logior index (ash 1 i))))))
    (if (and (not straddle) (aref (onode-children n) index))
	(insert-object s (aref (onode-children n) index))
	(push s (onode-olist n)))))

(defmethod determine-visible-set ((culler basic-culler) (root octree)) ;; Was (root r-tree)
  (labels ((build-visible-set (octants)
	     (let ((vset (iter (for o in octants)
			       (when (visible-p culler o)
				 (collect c)))))
	       (multiple-value-bind (nodes leaves) (aux:partition-if #'(lambda (x)
									 (typep x 'node))
								     (aux:mklist vset))
		 (append leaves
			 (aux:flatten (iter (for n in nodes)
					    (collecting (build-visible-set n)))))))))
    (when (visible-p culler root)
      (nreverse (build-visible-set (children root))))))

(defun test-octree1 ()
  (let ((boxes (iter (for i below 8)
		     (collecting (make-box (vector (random 1.0) (random 1.0) (random 1.0))
					   :scale-x 0.1 :scale-y 0.1 :scale-z 0.1))))
	(tree (make-octree (vector 0.5 0.5 0.5) 1.0 1)))
    (dolist (b boxes)
      (format t "Inserting object at ~A.~%" (center b))
      (insert-object b tree))
    tree))

;;; Morton Keys and linear octress

(defclass linear-octree (node)
  ((half-width :accessor root-half-width :initform 32.0 :initarg :half-width)
   (max-depth :accessor max-depth :initform 6 :initarg :max-depth)))


(defun make-linear-octree (center half-width depth)
  (let ((o (make-transform :b center)))
    (make-instance 'linear-octree 
		   :transform o 
		   :half-width half-width 
		   :max-depth depth
		   :children (make-hash-table :size 8 :rehash-size 8))))

(defmethod find-object ((o spatial) (octree linear-octree))
  (let ((o (center o)))
    (gethash (morton3 (x o) (y o) (z o)) (children octree))))

(defmethod find-object ((v vector) (octree linear-octree))
  (gethash (morton3 (x v) (y v) (z v)) octree))

(defmethod find-object ((k fixnum) (octree linear-octree))
  (gethash k octree))

(defmethod insert-object ((o spatial) (octree linear-octree))
  (let* ((c (center o))
	 (key (morton3 (x c) (y c) (z c))))
    (multiple-value-bind (node foundp) (gethash  key (children octree))
      (cond (foundp (push o (lonode-olist node)))
	    (t (let ((node (make-lonode :center c
					:key key
					:has-child 0))
		     (parent-key ))
		 ;; Find the parent
		 ))))))

(defun compute-parent-key (key)
  (ash key -3))

(defun compute-children-keys (key)
  (let ((keys (iter (for i below 8)
		    (collecting (+ (ash key 3) i)))))
    keys)) ;; Do something with has-child!

(defun node-depth (key)
  ;; Set a sentinel bit
  (setf (ldb (byte 1 (1+ (integer-length key))) key) 1)
  (iter (for d from 0)
	(for limit below 10)
	(until (= key 1))
	(format t "~A ; " key)
	(setf key (ash key -3))))

(defun morton3 (x y z)
  (+ (ash (part1by2 (floor x)) 2) (ash (part1by2 (floor y)) 1) (part1by2 (floor z))))

(defun part1by2 (n)
  (setf n (logand (logxor n (ash n 16)) #xff0000ff)
	n (logand (logxor n (ash n 8)) #x0300f00f)
	n (logand (logxor n (ash n 4)) #x030c30c3)
	n (logand (logxor n (ash n 2)) #x09249249)))

(defun test-morton3 ()
  (format t "(0,0,0)=~A~%" (morton3 0 0 0))
  (format t "(64,32,1)=~A~%" (morton3 64 32 1))
  (format t "(5.0,3.2312,68.3820)=~A~%" (morton3 5.0 3.2312 68.3820)))
